clear;
clc;

% parameter for homotopy
% X0 = [0.54232, -0.321, 0]';
X0 = [-0.707027, -0.688818, 0]';
% initial value
x0 = X0;
n = length(x0);
% initial step length
delta = 0.01;
% tolerance for Newton iteration
tol = 0.0001;


% homotopy_path record points in the homotopy path
% record is the number of homotopy_path
record = 1;
homotopy_path = x0';
% all_points record all points including angle > 18
% and count is the number of all_points
count = 0;
% calculate targent vector
tangent = getangent(@func, x0, X0);
while true
    % Euler method
    x = x0 + delta * tangent;
    if x(n) > 1
        delta = (1 - x0(n)) / tangent(n);
        x = x0 + delta * tangent;
    end
    if x(n) == 1
        v = zeros(n, 1);
        v(n) = 1;
    else
        v = tangent;
    end

    % Newton iteration
    [flag, newx] = newtoniter(@func, x, v, tol);
    % Newton iteration succeed, update x, half the step length delta and continue if fail
    if flag == 1
        x = newx;
    else
        disp("减半步长");
        delta = delta / 2;
        if delta < eps
            error("同伦道路跟踪失败!");
        end
        continue
    end
    
    % new tangent
    newtangent = getangent(@func, x, X0);
    % record all points incuding angle theta > 18
    count = count + 1
    allpoints(count, :) = x';

    % modify step length
    theta = acos(dot(newtangent, tangent) / (norm(tangent) * norm(newtangent))) * 180 / pi;
    if theta > 18
        disp("减半步长");
        delta = delta / 2;
        if delta < eps
            error("同伦道路跟踪失败!");
        end
        continue
    elseif theta < 6
        disp("加倍步长");
        delta = delta * 2;
    end

    % record points on the homotopy path
    record = record + 1;
    homotopy_path(record, :) = x';
    x0 = x;
    tangent = newtangent;

    % stop when t = 1
    if x(n) == 1
        break;
    end
end

homotopy_path
X = homotopy_path(:, 1);
Y = homotopy_path(:, 2);
T = homotopy_path(:, 3);
plot3(X, Y, T, '*-');
% title("初始点为(0.54232,-0.321)的同伦路径")
title("初始点为(0.707027,-0.688818)的同伦路径")
xlabel("x")
ylabel("y")
zlabel("t")
box on;